﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using OpenTK;

namespace Common {
    public class QuaternionUtil {
        public static Quaternion RotationBetweenVectors(Vector3 start, Vector3 dest) {

            start = Vector3.Normalize(start);
            dest = Vector3.Normalize(dest);

            float cosTheta = Vector3.Dot(start, dest);
            Vector3 rotationAxis;

            if (cosTheta < -1 + 0.001f) {
                // special case when vectors in opposite directions :
                // there is no "ideal" rotation axis
                // So guess one; any will do as long as it's perpendicular to start
                // This implementation favors a rotation around the Up axis,
                // since it's often what you want to do.
                rotationAxis = Vector3.Cross(new Vector3(0.0f, 0.0f, 1.0f), start);
                if (Length2(rotationAxis) < 0.01) // bad luck, they were parallel, try again!
                    rotationAxis =Vector3.Cross(new Vector3(1.0f, 0.0f, 0.0f), start);

                rotationAxis = Vector3.Normalize(rotationAxis);
                return Quaternion.FromAxisAngle( rotationAxis, 180.0f);
            }

            // Implementation from Stan Melax's Game Programming Gems 1 article
            rotationAxis =Vector3.Cross(start, dest);

            float s = (float)Math.Sqrt((1 + cosTheta) * 2);
            float invs = 1 / s;

            return new Quaternion(
                rotationAxis.X * invs,
                rotationAxis.Y * invs,
                rotationAxis.Z * invs,
                s * 0.5f
            );
        }
        public static Quaternion LookAt(Vector3 direction, Vector3 desiredUp) {

            if (Length2(direction) < 0.0001f)
                return Quaternion.Identity;

            // Recompute desiredUp so that it's perpendicular to the direction
            // You can skip that part if you really want to force desiredUp
            Vector3 right = Vector3.Cross(direction, desiredUp);
            desiredUp = Vector3.Cross(right, direction);

            // Find the rotation between the front of the object (that we assume towards +Z,
            // but this depends on your model) and the desired direction
            Quaternion rot1 = RotationBetweenVectors(new Vector3(0.0f, 0.0f, 1.0f), direction);
            // Because of the 1rst rotation, the up is probably completely screwed up. 
            // Find the rotation between the "up" of the rotated object, and the desired up
            Vector3 newUp = rot1 *new  Vector3(0.0f, 1.0f, 0.0f);
            Quaternion rot2 = RotationBetweenVectors(newUp, desiredUp);

            // Apply them
            return rot2 * rot1; // remember, in reverse order.
        }
        public static float Length2(Vector3 x) {
            return Vector3.Dot(x, x);
        }
        public static Quaternion RotateTowards(Quaternion q1, Quaternion q2, float maxAngle) {

            if (maxAngle < 0.001f) {
                // No rotation allowed. Prevent dividing by 0 later.
                return q1;
            }

            float cosTheta = Dot(q1, q2);

            // q1 and q2 are already equal.
            // Force q2 just to be sure
            if (cosTheta > 0.9999f) {
                return q2;
            }

            // Avoid taking the long path around the sphere
            if (cosTheta < 0) {
                q1 = q1 * -1.0f;
                cosTheta *= -1.0f;
            }

            float angle =(float) Math.Acos(cosTheta);

            // If there is only a 2° difference, and we are allowed 5°,
            // then we arrived.
            if (angle < maxAngle) {
                return q2;
            }

            // This is just like slerp(), but with a custom t
            float t = maxAngle / angle;
            angle = maxAngle;

            Quaternion res = ((float)Math.Sin((1.0f - t) * angle) * q1 + (float)Math.Sin(t * angle) * q2) *(1/ (float)Math.Sin(angle));
            res =Quaternion.Normalize(res);
            return res;

        }
        public static float Dot(Quaternion q1, Quaternion q2) {
            return q1.X * q2.X + q1.Y * q2.Y + q1.Z * q2.Z + q1.W * q2.W;
        }


    }
}
